#include "stdafx.h"
#include "3dApp.h"
#include "icosahedron.h"
#include "pcaApp.h"
#include "glutDraw.h"
#include <iostream>
#include <fstream>
#include <string>
#include <math.h>
#include <vector>
#include <GL/glut.h>
#define PI 3.1415926535897932384626433832795

using namespace std;

Object model; Ico ico;

//variable for PCA method
float cov[3][3], eigenValue[3], eigenVector[3][3], meanVertex[3];

//glut variables
GLfloat	light_position[] = {1.0, 1.0, 10.0, 0.0 }, white_light[] = { 1, 0.5, 0.9, 1.0 }, lmodel_ambient[] = {0.9, 0.9, 0.9, 1.0 };
int procMode=100;

fstream file; string inFile, outFile;
int depthIco =3, fcCount;
float Bu[3],Bv[3],Bw[3],rangeExp=1e9;
int maxSumAreaId;
float orthoLimit;
vector <int> nMaxSumAreaId;

void myKey(unsigned char key, int x, int y)
{
	switch(key) 
	{
	case 'w': 
		cout << "key 'w' is pressed! draw the object in wireframe" << endl;
		obj_mode = OBJ_WIREFRAME;
		break; 
	case 'f':
		cout << "key 'f' is pressed! draw the object in solid" << endl;
		obj_mode = OBJ_FLAT;
		break;
	case 's':
		cout << "key 's' is pressed! draw the object in solid+wireframe" << endl;
		obj_mode = OBJ_SMOOTH;
		break;
	case 'i':
		cout << "key 's' is pressed! draw icosahedron" << endl;
		obj_mode = 1;
		break;
	case '0':
		cout << "key '0' is pressed! draw the initial model" << endl;
		procMode = 0;
		glPopMatrix();
		break;
	case '1':
		cout<<"key '1' is pressed! aligning the model based on pca"<<endl;
		procMode = 1;
		break;
	case '2':
		cout<<"key '2' is pressed! alligning the model based on vca"<<endl;
		procMode=2;
    }
	// force the redraw function
	glutPostRedisplay(); 
}
void drawIco(){
	//draw the model in Wireframe view
	glDisable(GL_LIGHTING);
	glDisable(GL_LIGHT0);
	glColor3f(1.0,0,0);
		for(int i = 0; i<fcCount; i++)
		{
		glBegin(GL_LINES);
		glVertex3fv(ico.facesIco[i].v1);
		glVertex3fv(ico.facesIco[i].v2);
		glVertex3fv(ico.facesIco[i].v2);
		glVertex3fv(ico.facesIco[i].v3);
		glVertex3fv(ico.facesIco[i].v3);
		glVertex3fv(ico.facesIco[i].v1);
		glEnd();
		}
}

/*routine to read 'OFF' file format then store vertex position and faces information of the model*/
void readOFF(string fileName, Object &obj){
	file.open(fileName.c_str());
	if(!file)
	{
		cout << "Error in opening file" <<endl;
	}
	else
		cout << "File opened successfully" << endl;

	Vertex tmpVertex; Face tmpFace;
	int numVertex, numPoly;
	int n; //indicator of how many vertex in 1 polygon
	file.ignore(3);
	file >> numVertex; 
	file >> numPoly;
	file.ignore(4);

	for (int i=0;i<numVertex;i++)
	{
		
		file>>tmpVertex.v[0];
		file>>tmpVertex.v[1];
		file>>tmpVertex.v[2];
		tmpVertex.v[4]=0;
		obj.vertices.push_back(tmpVertex);
		/*model.vertices[i].v[X]=vx;
		file>>model.vertices[i].v[Y];
		file>>model.vertices[i].v[Z];
		model.vertices[i].v[W] = 0;*/
	}

	for(int i=0;i<numPoly;i++)
	{	
		file >> n;
		if (n==3)
		{
			file >> tmpFace.v1;
			file >> tmpFace.v2;
			file >> tmpFace.v3;
			obj.faces.push_back(tmpFace);
		}
	}
	file.close();
	file.clear();
}

/* routine to compute normal and area of each face of the model*/
void normal_areaComp(Object &obj)
{
	for(int i = 0; i < obj.faces.size(); i++)
	{
		Vertex &v1 = obj.vertices[obj.faces[i].v1];
		Vertex &v2 = obj.vertices[obj.faces[i].v2];
		Vertex &v3 = obj.vertices[obj.faces[i].v3];	
		Vector v12(v1.v[X]-v2.v[X],v1.v[Y]-v2.v[Y],v1.v[Z]-v2.v[Z],0);
		Vector v32(v3.v[X]-v2.v[X],v3.v[Y]-v2.v[Y],v3.v[Z]-v2.v[Z],0);
		Vector v13(v1.v[X]-v3.v[X],v1.v[Y]-v3.v[Y],v1.v[Z]-v3.v[Z],0);
		//compute normal vector of triangle using cross product from two vectors v21 & v31
		Vector normal(v12[Z]*v32[Y]-v12[Y]*v32[Z],
					v12[X]*v32[Z]-v12[Z]*v32[X],
					v12[Y]*v32[X]-v12[X]*v32[Y],0);
		assign(obj.faces[i].normal,normal);
		normalize(obj.faces[i].normal);
		//compute area of triangle
		Vector v12x13;
		cross(v12x13,v12,v13);
		obj.faces[i].area=0.5*length(v12x13);
	}
}

void translateCenter(Object &obj){
	int i,j; Vertex tmpV;
	obj.newVertices.clear();
	for(i=0;i<3;i++)
	{
		float temp = 0.f;
		for(j=0;j<obj.vertices.size();j++)
			temp+=obj.vertices[j].v[i];
		meanVertex[i]=temp/obj.vertices.size();
	}
	for(j=0;j<obj.vertices.size();j++)
	{
		for(i=0;i<3;i++)
			tmpV.v[i] = obj.vertices[j].v[i]-meanVertex[i];
		tmpV.v[3]=0;
		obj.newVertices.push_back(tmpV);
	}
}

void covarianceCalc(Object &obj){
	int i,j;
	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			cov[i][j]=0;
	for(i=0;i<obj.newVertices.size();i++)
	{
		cov[0][0]+=pow(obj.newVertices[i].v[0],2);
		cov[0][1]+=obj.newVertices[i].v[0]*obj.newVertices[i].v[1];
		cov[0][2]+=obj.newVertices[i].v[0]*obj.newVertices[i].v[2];
		
		cov[1][0]+=obj.newVertices[i].v[0]*obj.newVertices[i].v[1];
		cov[1][1]+=pow(obj.newVertices[i].v[1],2);
		cov[1][2]+=obj.newVertices[i].v[1]*obj.newVertices[i].v[2];

		cov[2][0]+=obj.newVertices[i].v[0]*obj.newVertices[i].v[2];
		cov[2][1]+=obj.newVertices[i].v[1]*obj.newVertices[i].v[2];
		cov[2][2]+=pow(obj.newVertices[i].v[2],2);
	}
}

void rewrite(string out, Object &obj){
	ofstream newFile;
	newFile.open(out.c_str());
	newFile<<"OFF"<<endl;
	newFile<<model.vertices.size()<<" "<<model.faces.size()<<" "<<0<<endl;
	for(int i=0;i<model.vertices.size();i++)
		newFile<<model.newVertices[i].v[X]<<" "<<model.newVertices[i].v[Y]<<" "<<model.newVertices[i].v[Z]<<endl;
	for(int i=0;i<model.faces.size();i++)
		newFile <<3 <<" "<<model.faces[i].v1<<" "<<model.faces[i].v2<<" "<<model.faces[i].v3<<endl;
	newFile.close();
	newFile.clear();
}

void initIco(Ico &ico, int depthIco, int &fcCount ){
	fcCount =0;
	int i;
	//initiate points for icosahedron
	for (i=0; i < 20; i++) 
	{
		subdivide(&vdata[tindices[i][0]][0],
		&vdata[tindices[i][1]][0],
		&vdata[tindices[i][2]][0],depthIco, ico, fcCount);
	}
	//calculate normal vector and plane equation variable for each face
	float v12[3], v32[3];
	for(i=0; i<fcCount;i++)
	{
		for(int j=0;j<3;j++)
		{
			v12[j] = ico.facesIco[i].v1[j]-ico.facesIco[i].v2[j];
			v32[j] = ico.facesIco[i].v3[j]-ico.facesIco[i].v2[j];
		}
		//normal vector
		ico.facesIco[i].n[0]= v12[2]*v32[1]-v12[1]*v32[2];
		ico.facesIco[i].n[1]= v12[0]*v32[2]-v12[2]*v32[0];
		ico.facesIco[i].n[2]= v12[1]*v32[0]-v12[0]*v32[1];
		//plane variable
		ico.facesIco[i].planeVar[0]=ico.facesIco[i].v1[1]*(ico.facesIco[i].v2[2]-ico.facesIco[i].v3[2])
									+ico.facesIco[i].v2[1]*(ico.facesIco[i].v3[2]-ico.facesIco[i].v1[2])
									+ico.facesIco[i].v3[1]*(ico.facesIco[i].v1[2]-ico.facesIco[i].v2[2]);
		ico.facesIco[i].planeVar[1]=ico.facesIco[i].v1[2]*(ico.facesIco[i].v2[0]-ico.facesIco[i].v3[0])
									+ico.facesIco[i].v2[2]*(ico.facesIco[i].v3[0]-ico.facesIco[i].v1[0])
									+ico.facesIco[i].v3[2]*(ico.facesIco[i].v1[0]-ico.facesIco[i].v2[0]);
		ico.facesIco[i].planeVar[2]=ico.facesIco[i].v1[0]*(ico.facesIco[i].v2[1]-ico.facesIco[i].v3[1])
									+ico.facesIco[i].v2[0]*(ico.facesIco[i].v3[1]-ico.facesIco[i].v1[1])
									+ico.facesIco[i].v3[0]*(ico.facesIco[i].v1[1]-ico.facesIco[i].v2[1]);
		ico.facesIco[i].planeVar[3]=-1*(ico.facesIco[i].v1[0]*(ico.facesIco[i].v2[1]*ico.facesIco[i].v3[2]-ico.facesIco[i].v3[1]*ico.facesIco[i].v2[2])
									+ico.facesIco[i].v2[0]*(ico.facesIco[i].v3[1]*ico.facesIco[i].v1[2]-ico.facesIco[i].v1[1]*ico.facesIco[i].v3[2])
									+ico.facesIco[i].v3[0]*(ico.facesIco[i].v1[1]*ico.facesIco[i].v2[2]-ico.facesIco[i].v2[1]*ico.facesIco[i].v1[2]));
	}
}

int pointInTriangle(float *a, float *b, float *c, float *p){
	float ba[3],ca[3],pa[3];
	float dot00=0, dot01=0, dot02=0, dot11=0, dot12=0;
	float invDenom,u,v;
	for (int i=0;i<3;i++)
	{
		ca[i] = c[i]-a[i];
		ba[i] = b[i]-a[i];
		pa[i] = p[i]-a[i];
	}
	for(int i=0;i<3;i++)
	{
		dot00+=ca[i]*ca[i];
		dot01+=ca[i]*ba[i];
		dot02+=ca[i]*pa[i];
		dot11+=ba[i]*ba[i];
		dot12+=ba[i]*pa[i];
	}
	invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
	u = (dot11 * dot02 - dot01 * dot12) * invDenom;
	v = (dot00 * dot12 - dot01 * dot02) * invDenom;
	int inTriangle = (u > 0) && (v > 0) && (u + v < 1);
	return inTriangle;
}

void clusterIco(Ico &ico, Object &obj)
{
	//detect intersection between normal vector of model with plane of icosahedron faces
	float nDotV, time, intersect[3];
	int i,a;
	for (i=0; i<ico.facesIco.size();i++)
	{
		for (a=0; a<obj.faces.size();a++)
		{
			nDotV=obj.faces[a].normal[0]*ico.facesIco[i].planeVar[0]+
				obj.faces[a].normal[1]*ico.facesIco[i].planeVar[1]+
				obj.faces[a].normal[2]*ico.facesIco[i].planeVar[2];
			if(nDotV!=0)
			{	
				time = -1*ico.facesIco[i].planeVar[3]/nDotV;
				if(time>0 && time<1)//better condition checking is to limit the ray into segment so there will be less 'point in triangle' checking
				{
					//compute intersection point
					for(int x=0;x<3;x++)
						intersect[x]=time*obj.faces[a].normal[x];
					//check if the intersection is inside the triangle
					int inTrigle=pointInTriangle(ico.facesIco[i].v1,ico.facesIco[i].v2,ico.facesIco[i].v3,intersect);
					if(inTrigle)
						ico.facesIco[i].fcModelId.push_back(a);
				}
			}
		}
	}
	//check if number of normals allocated = number of faces
	int numNormal=0;
	for(int i=0;i<ico.facesIco.size();i++)
	{
		numNormal += ico.facesIco[i].fcModelId.size();
	}
	cout<< numNormal<<" faces"<<endl;	

	//sum model face's area under same icosahedron distribution
	for (i=0; i<ico.facesIco.size();i++)
	{
		ico.facesIco[i].sumArea=0;
		if(ico.facesIco[i].fcModelId.size()>0)
		{
			for(int j=0;j<ico.facesIco[i].fcModelId.size();j++)
			{
				int index=ico.facesIco[i].fcModelId[j];
				ico.facesIco[i].sumArea+=obj.faces[index].area;
			}
		}
	}
}
void firstAxis(Ico &ico){
	//find maximum index of the previously computed sum-area distribution
	int i;
	float sumAreaMax=0;
	for(i=0;i<ico.facesIco.size();i++)
	{
		if(ico.facesIco[i].sumArea>sumAreaMax)
		{
			sumAreaMax = ico.facesIco[i].sumArea;
			maxSumAreaId = i;
		}
	}
	for(i=0;i<3;i++)
		Bu[i]=ico.facesIco[maxSumAreaId].n[i];
	normalize3(Bu);
}

void returnNLargestID(vector<int> &id, vector<float> &data, int n){
	vector <float>data1=data;
	while(id.size()<n)
	{
		float a = -1e35;int ind;
		for(int i=0;i<data1.size();i++)
		{
			if(data1[i]>a)
			{
				a=data1[i];
				ind=i;
			}
		}
		data1[ind]=-1e-35;
		id.push_back(ind);
	}
}

void returnNMinimumID(vector<int> &id, vector<float> &data, int n){
	vector <float>data1=data;
	while(id.size()<n)
	{
		float a = 1e35;int ind;
		for(int i=0;i<data1.size();i++)
		{
			if(data1[i]<a)
			{
				a=data1[i];
				ind=i;
			}
		}
		data1[ind]=1e35;
		id.push_back(ind);
	}
}

void secondAxisOrtho(Ico &ico){
	int i, secondAxisId;
	vector <int> orthoSetsId;	vector <float> ortho;

	//find the orthogonal normal sets wrt the first maximum sum-area (that have dot product under 'orthoLimit')
	for(i=0;i<ico.facesIco.size();i++)
	{
		float dot=dot3(ico.facesIco[maxSumAreaId].n,ico.facesIco[i].n);
		if(fabs(dot)<orthoLimit)
		{
			orthoSetsId.push_back(i);
			ortho.push_back(dot);
		}
	}

	//select 'n=10' minimum dot product from 'ortho' vector set
	vector <int> tmpOrthoID;
	returnNMinimumID(tmpOrthoID,ortho,10);

	//find direction with maximum area under 'n=10' orthogonal normal set
	float tempArea=-1;
	for(i=0;i<tmpOrthoID.size();i++)
	{
		int c=orthoSetsId[tmpOrthoID[i]];
		if(ico.facesIco[c].sumArea>tempArea)
			secondAxisId=c;
	}
	for(i=0;i<3;i++)
	{
		Bv[i]=ico.facesIco[secondAxisId].n[i];
	}
}

void secondAxisArea(Ico &ico){
	int i,j;
	vector <float> dataArea; vector <int> tmpMaxAreaId;
	vector<float> ortho;
	
	//find 'n=10' largest sumArea (note first axis is the largest from distribution)
	for(i=0;i<ico.facesIco.size();i++)
		dataArea.push_back(ico.facesIco[i].sumArea);
	returnNLargestID(tmpMaxAreaId,dataArea,11);
	tmpMaxAreaId.erase(tmpMaxAreaId.begin());

	//calculate dot product wrt first axis out of the previous sample
	for(i=0; i<tmpMaxAreaId.size();i++)
	{
		j=tmpMaxAreaId[i];
		ortho.push_back(dot3(Bu,ico.facesIco[j].n));
	}

	//find the smallest dot product out of the previous sample
	float tmp=1e40; int ind;
	for(i=0;i<tmpMaxAreaId.size();i++)
		if(ortho[i]<tmp)
		{
			tmp = ortho[i];
			ind = tmpMaxAreaId[i];
		}

	//assign the second axis
	for(i=0;i<3;i++)
		Bv[i]=ico.facesIco[ind].n[i];
}

void orthogonalize(float *result, float *a, float *b)
{
	float k = -1*dot3(a,b)/pow(length3(a),2);
	addScaled3(result,a,b,k);
	normalize3(Bv);
}

void findCenter(Object &obj)
{
	int i,j;
	float sumXCenter[3]={0,0,0}, totalArea=0;
	for(i=0;i<obj.faces.size();i++)
	{
		for(j=0;j<3;j++)
		{
			obj.faces[i].c[j]=1/3*(obj.vertices[obj.faces[i].v1].v[j]+
									obj.vertices[obj.faces[i].v2].v[j]+
									obj.vertices[obj.faces[i].v3].v[j]);
			sumXCenter[j]+=obj.faces[i].area*obj.faces[i].c[j];
		}
		totalArea+=obj.faces[i].area;
	}
	for(i=0;i<3;i++)
		obj.center[i] = 1/totalArea*sumXCenter[i];
}

void calcNewVertices(Object &obj)
{
	int i,j;
	float tmpTransV[3], vert[3];
	float fake[3]={0,0,0};
	Vertex tmpV;
	obj.newVertices.clear();
	for(i=0;i<obj.vertices.size();i++)
	{
		for(j=0;j<3;j++)
		{
			vert[j]=obj.vertices[i].v[j];
		}
		sub3(tmpTransV,vert,fake);
		tmpV.v[0]=dot3(tmpTransV,Bu);
		tmpV.v[1]=dot3(tmpTransV,Bv);
		tmpV.v[2]=dot3(tmpTransV,Bw);
		/*tmpV.v[0]=tmpTransV[0]*Bu[0]+tmpTransV[0]*Bv[0]+tmpTransV[0]*Bw[0];
		tmpV.v[1]=tmpTransV[1]*Bu[1]+tmpTransV[1]*Bv[1]+tmpTransV[1]*Bw[1];
		tmpV.v[2]=tmpTransV[2]*Bu[2]+tmpTransV[2]*Bv[2]+tmpTransV[2]*Bw[2];*/
		tmpV.v[3]=0;
		obj.newVertices.push_back(tmpV);
	}
}

void disp()
{
	glEnable(GL_DEPTH_TEST); 
	glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity(); 
	gluPerspective(60, aspect_ratio, 0.1,100);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity(); 
	gluLookAt(0,0,3,0,0,0,0,1,0); 
	//rotate and scale the object
	glPushMatrix();
	glRotatef(x_angle, 0, 1,0); 
	glRotatef(y_angle, 1,0,0); 
	glScalef(scale_size, scale_size, scale_size); 
	glTranslatef(x_meas,y_meas,0);

	glLightfv(GL_LIGHT0, GL_POSITION, light_position);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, white_light);
	glLightfv(GL_LIGHT0, GL_SPECULAR, white_light);
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, lmodel_ambient);
//	if(obj_mode==1)drawIco();
	if(procMode==0)	
	{
		glPopMatrix();
		drawModel(model);
		procMode=100;
	}
	else if(procMode==1)
	{
		glPopMatrix();
		translateCenter(model);
		covarianceCalc(model);
		computeRoots(cov,eigenValue);
		eigenVectorCalc(eigenValue,eigenVector,cov);
		for(int i=0;i<3;i++)
		{
			Bu[i]=eigenVector[0][i];
			Bv[i]=eigenVector[1][i];
			Bw[i]=eigenVector[2][i];
		}
		calcNewVertices(model);
		glColor3f(1,0,0);
		drawNewModel(model);
		procMode=10;
	}
	else if(procMode==2)
	{	
		glPopMatrix();
		clusterIco(ico, model);
		firstAxis(ico);
		cout<<"Bu :";dumpFloat3(Bu);

		//secondAxisOrtho(ico);
		secondAxisArea(ico);
		orthogonalize(Bv,Bu,Bv);
		cout<<"Bv :";dumpFloat3(Bv);

		double c = dot3(Bu,Bv);
		cross3(Bw,Bu,Bv);
		normalize3(Bw);
		cout<<"Bw :";dumpFloat3(Bw);

		double d = dot3(Bu,Bw);
		findCenter(model);
		calcNewVertices(model);
		rewrite("m000.off", model);
		glColor3f(0,0,1);
		drawNewModel(model);
		procMode=20;
	}
	else if(procMode==20)drawNewModel(model);
	else if(procMode==10)drawNewModel(model);
	else if(procMode==100)drawModel(model);
	glutSwapBuffers();
}
			
int main(int argc,char *argv[])
{
	cout << "Please enter the input file name: "<<endl;
	//cin >> inFile;
	inFile = "cone.off";
	readOFF(inFile,model);
	normal_areaComp(model);
	
	depthIco= 2;
	initIco(ico, depthIco, fcCount);
	orthoLimit=1e-4;

/* coz we love to draw*/
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE );
	glutInitWindowSize(900,900);
	glutInitWindowPosition(100,100);
	window = glutCreateWindow("icosahedron");
	glClearColor(0.9,0.9,0.9,0.0);
	// register for callback functions
	glutDisplayFunc(disp);			//for display
	glutMouseFunc(myMouse);			//for mouse movements
	glutMotionFunc(myTransform);	//for motions
	glutKeyboardFunc(myKey);		//for keyboard
	glutReshapeFunc(reshape);		//for reshape
	glutMainLoop();

}